import cv2 as cv
import numpy as np
import math

transformed_points = []
Reference_point_x = 0
Reference_point_y = 0
def forward_direction(img):
    global transformed_points
    global Reference_point_x
    global Reference_point_y
    # img = cv.resize(img, None, fx=1.3, fy=1.3, interpolation=cv.INTER_CUBIC)
    # 旋转45度
    # rows, cols ,_= img.shape
    # # cols-1 和 rows-1 是坐标限制
    # M1 = cv.getRotationMatrix2D(((cols - 1) / 2.0, (rows - 1) / 2.0), 45, 1)
    # img = cv.warpAffine(img, M1, (cols, rows))
    # M2 = cv.getRotationMatrix2D(((cols - 1) / 2.0, (rows - 1) / 2.0), -45, 1)
    # img = cv.warpAffine(img, M2, (cols, rows)
    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)  # 转化为灰度图像
    blur = cv.GaussianBlur(gray, (7, 7), 0)  # 高斯模糊
    _, frame_bin = cv.threshold(gray, 0, 255, cv.THRESH_BINARY | cv.THRESH_OTSU)  # 进行二值化
    kernel = np.ones((7, 7), np.uint8)
    dilation = cv.erode(frame_bin, kernel, iterations=2)


    edges = cv.Canny(dilation, 100, 200)  # Canny查找边缘
    _, contours, hierarchy = cv.findContours(edges, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)  # 边缘检测
    cv.imshow("name", frame_bin)
    vertex = []  # 储存矩形的四个顶点
    max_area = 50000  # 定义矩形最大面积
    min_area = 1000
    EPSILON_RATIO = 0.02  # 定义边长缩放比例
    # 判断是不是矩形
    if len(contours) == 0:
        return
    # print("*" * 10)
    for i in range(0, len(contours)):  #
        area = cv.contourArea(contours[i])
        # print("area is:", area)
        if cv.contourArea(contours[i]) <= max_area and cv.contourArea(contours[i]) >= min_area:
            epsilon = EPSILON_RATIO * cv.arcLength(contours[i], True)
            approx = cv.approxPolyDP(contours[i], epsilon, True)
            if len(approx) == 4:
                vertex.append(approx)

    # 获取中心点坐标
    centers = []
    for i in vertex:
        # 计算轮廓的边界框
        x, y, w, h = cv.boundingRect(i)
        # 计算矩形的中心点坐标
        cx = x + w // 2
        cy = y + h // 2
        centers.append([cx, cy])

    # 获取距离小于或等于设定的阈值的点的下标对
    # 相似度阈值设定为百分之一
    threshold = 30
    # 存储满足条件的点对的下标
    similar_indices = []
    # 遍历每对点进行比较
    n = len(centers)
    i_list = []
    j_list = []
    for i in range(n):
        if i not in i_list:
            for j in range(i + 1, n):
                if j not in j_list:
                    # 计算欧氏距离
                    x1, y1 = centers[i]
                    x2, y2 = centers[j]
                    distance = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
                    # print("distance is:", distance)
                    # 判断距离是否在阈值内
                    if distance <= threshold:
                        similar_indices.append((i, j))
                        i_list.append(i)
                        j_list.append(j)
                        break

    # print("centers is :",centers)
    # print("len is:",len(vertex))
    # print(vertex)
    # print("similar_indices is:", similar_indices)

    Filtered_vertices = []
    Filtered_center_points = []
    # 从相似的下标对中找出单独的矩形和中心点
    for similar_points_index in similar_indices:
        # 选择每个相似对中的第0个
        Filtered_vertices.append(vertex[similar_points_index[0]])
        Filtered_center_points.append(centers[similar_points_index[0]])
        cv.circle(img, tuple(centers[similar_points_index[0]]), 10, (0, 0, 255), -1)

    # 将中心点坐标和对应的顶点坐标配对
    rectangles = list(zip(Filtered_center_points, Filtered_vertices))
    # 按照中心点坐标的从上到下
    rectangles_sorted = sorted(rectangles, key=lambda x: x[0][1])
    first_line_three = sorted(rectangles_sorted[:3], key=lambda x: x[0][0])
    middle_line_three = sorted(rectangles_sorted[3:6], key=lambda x: x[0][0])
    last_line_three = sorted(rectangles_sorted[6:9], key=lambda x: x[0][0])

    combined_list = first_line_three + middle_line_three + last_line_three

    # 提取排序后的顶点坐标列表和中心点坐标列表
    Sorted_vertices = [rect[1] for rect in combined_list]
    Sorted_center_points = [rect[0] for rect in combined_list]

    number = 0
    for i in Sorted_center_points:
        number += 1
        cv.putText(img, str(number), tuple(i), cv.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 2, cv.LINE_AA)
    # print("Sorted_vertices is:", Sorted_vertices)
    cv.drawContours(img, Filtered_vertices, -1, (0, 255, 0), 1)

    # 设置与步进电机相同的参考点Reference_point_x,Reference_point_y，选为左下点，及7的左下点
    if len(Sorted_center_points) == 9:
        Reference_point_x = 0
        Reference_point_y = 0
        Grid_seven_center_x, Grid_seven_center_y = Sorted_center_points[6]
        for i in Sorted_vertices[6]:
            if i[0][0] < Grid_seven_center_x and i[0][1] > Grid_seven_center_y:
                Reference_point_x = i[0][0]
                Reference_point_y = i[0][1]

        # print("Sorted_center_points[6]",Sorted_center_points[6])
        # print("Reference_point_x is:", Reference_point_x)
        # print("Reference_point_y is:", Reference_point_y)

        # 对所有中心点根据参考点进行坐标转化
        transformed_points = []
        for (x, y) in Sorted_center_points:
            transformed_x = x - Reference_point_x
            transformed_y = -(y - Reference_point_y)  # 取负号操作
            transformed_points.append((transformed_x, transformed_y))

        # print("transformed_points", transformed_points)
        # print("Reference_point_x is:", Reference_point_x)
        # print("Reference_point_y is:", Reference_point_y)
        # print("Sorted_center_points", Sorted_center_points)
    return img


